/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore;

import ore.errors.FrameAncestorException;
import ore.errors.OrekitException;
import ore.frames.FramesFactory;
import ore.time.AbsoluteDate;

import java.util.LinkedList;
import java.util.HashMap;
import java.util.Map;

/** 
 * A three dimensional inertial reference frame is a celestial body,
 * or a space craft inertial reference frame.
 *
 * <h5> Frame Presentation </h5>
 * 
 * This class is the base class for all frames in OREKIT. The frames
 * are linked together in a tree with some specific frame chosen as
 * the root of the tree.  Each frame is defined by {@link Transform
 * transforms} combining any number of translations and rotations from
 * a reference frame which is its parent frame in the tree structure.
 * 
 * When we say a {@link Transform transform} t is <em>from
 * frame<sub>A</sub> to frame<sub>B</sub></em>, we mean that if the
 * coordinates of some absolute vector (say the direction of a distant
 * star for example) has coordinates u<sub>A</sub> in
 * frame<sub>A</sub> and u<sub>B</sub> in frame<sub>B</sub>, then
 * u<sub>B</sub>={@link Transform#transformVector(Vector3D)
 * t.transformVector(u<sub>A</sub>)}.  <p>The transforms may be
 * constant or varying. For simple fixed transforms, using this base
 * class is sufficient. For varying transforms (time-dependent or
 * telemetry-based for example), it may be useful to define specific
 * subclasses that will implement {@link #updateFrame(AbsoluteDate)}
 * or that will add some specific
 * <code>updateFromTelemetry(telemetry)</code> methods that will
 * compute the transform and call internally the {@link
 * #setTransform(Transform)} method.
 *
 * @author Guylaine Prat
 * @author Luc Maisonobe
 * @author Pascal Parraud
 */
public class Frame
    extends Object
    implements java.io.Serializable
{
    /**
     * 2&pi;.
     */
    public final static double TWO_PI = 2.0 * Math.PI;
    /**
     * Seconds per day.
     */
    public final static double SECONDS_PER_DAY = 86400.0;
    /** 
     * Radians per second of time.
     */
    public final static double RADIANS_PER_SECOND = TWO_PI / SECONDS_PER_DAY;
    /** 
     * Radians per arcsecond.
     */
    public final static double RADIANS_PER_ARC_SECOND = Math.PI / 648000;
    /**
     * Julian century per second.
     */
    public final static double JULIAN_CENTURY_PER_SECOND = 1.0 / (36525.0 * SECONDS_PER_DAY);


    public final static Frame Root = new Frame("GCRF");

    /** 
     * The GCRF frame is the root frame in the frame tree.
     * @return The unique instance of the root frame
     */
    public final static Frame getRoot() {
        return Frame.Root;
    }
    /** 
     * The GCRF frame is the root frame in the frame tree.
     * @return The unique instance of the GCRF frame
     */
    public final static Frame getGCRF() {
        return Frame.Root;
    }
    /** 
     * The J2000 frame is also called the EME2000 frame.
     * The later denomination is preferred in Orekit.
     * @return the unique instance of the EME2000/J2000 frame
     */
    public final static Frame getJ2000() {
        return FramesFactory.getEME2000();
    }
    /** 
     * The EME2000 frame is also called the J2000 frame.  The former
     * denomination is preferred in Orekit.
     * @return The unique instance of the EME2000 frame
     */
    public final static Frame getEME2000() {
        return FramesFactory.getEME2000();
    }
    /** 
     * @return ITRF2005 reference frame singleton.
     * @exception OrekitException The precession-nutation model data
     * embedded in the library cannot be read.
     */
    public final static Frame getITRF2005()
        throws OrekitException
    {
        return FramesFactory.getITRF2005();
    }
    /** 
     * @return TIRF2000 reference frame singleton.
     * @exception OrekitException The precession-nutation model data
     * embedded in the library cannot be read.
     */
    public final static Frame getTIRF2000()
        throws OrekitException
    {
        return FramesFactory.getTIRF2000();
    }
    /** 
     * @return CIRF2000 reference frame singleton.
     * @exception OrekitException The precession-nutation model data
     * embedded in the library cannot be read.
     */
    public final static Frame getCIRF2000()
        throws OrekitException
    {
        return FramesFactory.getCIRF2000();
    }
    /** 
     * @return VEIS 1950 reference frame singleton.
     */
    public final static Frame getVeis1950() {
        return FramesFactory.getVeis1950();
    }

    /** 
     * Instance name.
     */
    public final String name;
    /** 
     * Parent frame, the root frame doesn't have a parent.
     */
    public final Frame parent;
    /** 
     * Transform from parent frame to instance.
     */
    private Transform transform;
    /** 
     * Map of deepest frames commons with other frames.
     */
    private final Map<Frame, Frame> commons = new HashMap<Frame, Frame>();;


    /** 
     * Root frame contructor, GCRF
     */
    private Frame(String name) {
        super();
        this.parent    = null;
        this.transform = Transform.IDENTITY;
        this.name = name;
    }
    /** 
     * The convention for the transform is that it is from parent to
     * child.  This means that the two following frames are
     * similar.
     * 
     * <pre>
     * Frame frame1 = new Frame(FramesFactory.getGCRF(), new Transform(t1, t2)); 
     * Frame frame2 = new Frame(new Frame(FramesFactory.getGCRF(), t1), t2);
     * </pre>
     * 
     * @param parent parent frame (must not be null)
     * @param transform transform from parent frame to instance
     * @param name name of the frame
     * 
     * @exception IllegalArgumentException if the parent frame is null
     */
    public Frame(Frame parent, Transform transform, String name)
        throws IllegalArgumentException
    {
        super();
        if (parent == null) {
            throw OrekitException.createIllegalArgumentException("null parent for frame {0}",
                                                                 name);
        }
        else {
            this.name      = name;
            this.parent    = parent;
            this.transform = transform;
        }
    }


    public final String getName() {
        return this.name;
    }
    public final String toString() {
        return this.name;
    }
    public final Frame getParent() {
        return this.parent;
    }
    /** 
     * @param potentialAncestor Ancestor frame
     * @return true The argument ancestor belongs to the path from
     * instance to the root frame
     */
    public final boolean isChildOf(Frame potentialAncestor) {
        for (Frame frame = parent; frame != null; frame = frame.parent) {
            if (frame == potentialAncestor) {
                return true;
            }
        }
        return false;
    }
    /** 
     * @param transform New transform from parent to this frame 
     */
    public final void setTransform(final Transform transform) {
        this.transform = transform;
    }
    /** 
     * @param destination Destination frame to which we want to transform vectors
     * @param date Current date (can be null if it is sure than no date dependent frame is used)
     * @return transform From the instance to the destination frame
     * 
     * @exception OrekitException if some frame specific error occurs
     */
    public final Transform getTransformTo(final Frame destination, final AbsoluteDate date)
        throws OrekitException
    {
        if (this == destination) {

            return Transform.IDENTITY;
        }
        else {
            /*
             * Common ancestor to both frames in the frames tree
             */
            final Frame common = FindCommon(this, destination);
            /*
             * Transform from common to instance
             */
            Transform commonToInstance = Transform.IDENTITY;
            for (Frame frame = this; frame != common; frame = frame.parent) {
                frame.updateFrame(date);
                commonToInstance =
                    new Transform(frame.transform, commonToInstance);
            }
            /*
             * Transform from destination up to common
             */
            Transform commonToDestination = Transform.IDENTITY;
            for (Frame frame = destination; frame != common; frame = frame.parent) {
                frame.updateFrame(date);
                commonToDestination =
                    new Transform(frame.transform, commonToDestination);
            }
            /*
             * Transform from instance to destination via common
             */
            return new Transform(commonToInstance.getInverse(), commonToDestination);
        }
    }
    /** 
     * Update the frame to the given date.
     * 
     * This method is called each time {@link #getTransformTo(Frame, AbsoluteDate)}
     * is called. The base implementation in the {@link Frame} class does nothing.
     * 
     * The proper way to build a date-dependent frame is to extend the {@link Frame}
     * class and implement this method which will have to call {@link
     * #setTransform(Transform)} with the new transform.
     * 
     * @param date New value of the  date
     * 
     * @exception OrekitException if some frame specific error occurs
     */
    protected void updateFrame(AbsoluteDate date) throws OrekitException {
    }
    /** 
     * Update the transform from parent frame implicitly according to
     * two other frames.
     * 
     * This method allows to control the relative position of two
     * parts of the global frames tree using any two frames in each
     * part as control handles. Consider the following simplified
     * frames tree as an example.
     * 
     * <pre>
     *              GCRF
     *                |
     *  --------------------------------
     *  |             |                |
     * Sun        satellite          Earth
     *                |                |
     *        on-board antenna   ground station
     *                                 |
     *                          tracking antenna
     * </pre>
     * 
     * Tracking measurements really correspond to the link between the
     * ground and on-board antennas. This is tightly linked to the
     * transform between these two frames, however neither frame is
     * the direct parent frame of the other ones: the path involves
     * four intermediate frames. When we process a measurement, what
     * we really want to update is the transform that defines the
     * satellite frame with respect to its parent GCRF frame. This is
     * the purpose of this method. This update is done by the
     * following call, where <code>measurementTransform</code>
     * represent the measurement as a simple translation transform
     * between the two antenna frames.
     * 
     * <pre>
     * satellite.updateTransform(onBoardAntenna, trackingAntenna,
     *                           measurementTransform, date);
     * </pre>
     * 
     * One way to represent the behavior of the method is to consider
     * the sub-tree rooted at the instance on one hand (satellite and
     * on-board antenna in the example above) and the tree containing
     * all the other frames on the other hand (GCRF, Sun, Earth,
     * ground station, tracking antenna).  Both tree are considered as
     * solid sets linked by a flexible spring, which is the transform
     * we want to update. The method stretches the spring to make sure
     * the transform between the two specified frames (one in each
     * tree part) matches the specified transform.
     * 
     * @param f1 first control frame (may be the instance itself)
     * @param f2 second control frame (may be the instance itself)
     * @param f1Tof2 desired transform from first to second control frame
     * @param date date of the transform
     * 
     * @exception OrekitException if the path between the two control
     * frames does not cross the link between instance and its parent
     * frame or if some intermediate transform fails
     * 
     * @see #setTransform(Transform)
     */
    public final void updateTransform(Frame f1, Frame f2, Transform f1Tof2,
                                      AbsoluteDate date)
        throws OrekitException
    {
        Frame fA = f1;
        Frame fB = f2;
        Transform fAtoB = f1Tof2;
        /*
         * Make sure f1 is not a child of the instance
         */
        if (fA.isChildOf(this) || (fA == this)) {

            if (fB.isChildOf(this) || (fB == this)) {
                throw new FrameAncestorException("both frames {0} and {1} are child of {2}",
                                                 fA.getName(), fB.getName(), getName());
            }
            /*
             * Swap f1 and f2 to make sure the child is f2
             */
            final Frame tmp = fA;
            fA = fB;
            fB = tmp;
            fAtoB = fAtoB.getInverse();

        } else if (!(fB.isChildOf(this) || (fB == this))) {
            throw new FrameAncestorException("neither frames {0} nor {1} have {2} as ancestor",
                                             fA.getName(), fB.getName(), getName());
        }
        /*
         * Rebuild the transform by traveling from parent to self,
         * WITHOUT using the existing 'this.transform' that will be
         * updated
         */
        Transform parentTofA = parent.getTransformTo(fA, date);
        Transform fBtoSelf   = fB.getTransformTo(this, date);
        Transform fAtoSelf   = new Transform(fAtoB, fBtoSelf);
        this.setTransform(new Transform(parentTofA, fAtoSelf));
    }
    /** 
     * @return Path from instance to root, excluding instance itself
     * (empty if instance is root)
     */
    private LinkedList<Frame> pathToRoot() {
        LinkedList<Frame> path = new LinkedList<Frame>();
        for (Frame frame = parent; frame != null; frame = frame.parent) {
            path.add(frame);
        }
        return path;
    }

    /** 
     * @param from origin frame
     * @param to destination frame
     * @return The deepest common ancestor frame of both
     * <code>from</code> and <code>to</code>
     */
    private static Frame FindCommon(Frame from, Frame to) {
        /*
         * Have we already computed the common frame for this pair ?
         */
        Frame common = (Frame) from.commons.get(to);
        if (common != null)
            return common;
        else {
            /*
             * Definitions of the path up to the head tree for each frame
             */
            LinkedList<Frame> pathFrom = from.pathToRoot();
            LinkedList<Frame> pathTo   = to.pathToRoot();

            if (pathFrom.isEmpty() || pathTo.contains(from)) {
                /*
                 * Handle root case and same branch case
                 */
                common = from;
                from.commons.put(to, common);
                to.commons.put(from, common);
                return common;
            }
            else if (pathTo.isEmpty() || pathFrom.contains(to)) {
                /*
                 * Handle root case and same branch case
                 */
                common = to;
                from.commons.put(to, common);
                to.commons.put(from, common);
                return common;
            }
            else {
                /*
                 * At this stage pathFrom contains at least one frame
                 */
                Frame lastFrom = (Frame) pathFrom.removeLast();

                common = lastFrom; /* Common must be one of the
                                    * instance of Frame already
                                    * defined
                                    */
                /*
                 * At the beginning of the loop pathTo contains at
                 * least one frame
                 */
                for (Frame lastTo = (Frame) pathTo.removeLast();
                     (lastTo == lastFrom) && (lastTo != null) && (lastFrom != null);
                     lastTo = (Frame) (pathTo.isEmpty() ? null : pathTo.removeLast()))
                {
                    common = lastFrom;
                    lastFrom = (Frame) (pathFrom.isEmpty() ? null : pathFrom.removeLast());
                }

                from.commons.put(to, common);
                to.commons.put(from, common);
                return common;
            }
        }
    }
}
